/// \file tests.cpp
/// \brief test file
#define CATCH_CONFIG_MAIN
#include "catch.hpp"

#include "turtlelib/rigid2d.hpp"

#include <cmath>
#include <iostream>
#include <sstream>

TEST_CASE("Test function almost_equal(d1, d2, epsilon)") {

  REQUIRE(turtlelib::almost_equal(0, 0));
  REQUIRE(turtlelib::almost_equal(0.001, 0.005, 1.0e-2));
  REQUIRE(turtlelib::almost_equal(0.1, 0.5, 1.0));
  // REQUIRE( turtlelib::almost_equal(1, 500));  // This test should fail
}

TEST_CASE("Test function deg2rad(deg)") {

  REQUIRE(turtlelib::almost_equal(turtlelib::deg2rad(0.0), 0.0));
  REQUIRE(turtlelib::almost_equal(turtlelib::deg2rad(180.0), turtlelib::PI));
  // REQUIRE( turtlelib::almost_equal(turtlelib::deg2rad(360.0), turtlelib::PI));  //
  // This test should fail
}

TEST_CASE("Test function rad2deg(rad)") {

  REQUIRE(turtlelib::almost_equal(turtlelib::rad2deg(0.0), 0.0));
  REQUIRE(turtlelib::almost_equal(turtlelib::rad2deg(turtlelib::PI), 180.0));
  // REQUIRE( turtlelib::almost_equal(turtlelib::rad2deg(turtlelib::PI), 360.0));  //
  // This test should fail
}

TEST_CASE("Test functions deg2rad(deg) and rad2deg(rad)") {

  REQUIRE(turtlelib::almost_equal(turtlelib::deg2rad(turtlelib::rad2deg(0.0)), 0.0));
  REQUIRE(turtlelib::almost_equal(turtlelib::deg2rad(turtlelib::rad2deg(2.1)), 2.1));
  // REQUIRE( turtlelib::almost_equal(turtlelib::rad2deg(180.0), 360.0));  // This
  // test should fail
}

TEST_CASE("Test Transform2D function inv()") {

  double angle = turtlelib::PI;
  turtlelib::Vector2D v;
  v.x = 1;
  v.y = 1;
  turtlelib::Transform2D T_ident;
  turtlelib::Transform2D T(v, angle);
  turtlelib::Transform2D T_inv = T.inv();

  REQUIRE(turtlelib::almost_equal((T * T_inv).translation().x, T_ident.translation().x));
  REQUIRE(turtlelib::almost_equal((T * T_inv).translation().y, T_ident.translation().y));
  REQUIRE(turtlelib::almost_equal((T * T_inv).rotation(), T_ident.rotation()));
}

TEST_CASE("Test Transform2D function Transform2D(v, angle)") {

  double angle = 0.0;
  turtlelib::Vector2D v;
  v.x = 0.0;
  v.y = 0.0;
  turtlelib::Transform2D T_ident;
  turtlelib::Transform2D T(v, angle);

  REQUIRE(turtlelib::almost_equal(T.translation().x, T_ident.translation().x));
  REQUIRE(turtlelib::almost_equal(T.translation().y, T_ident.translation().y));
  REQUIRE(turtlelib::almost_equal(T.rotation(), T_ident.rotation()));
}

